from ctypes import *
import platform
from os.path import dirname, join
import numpy as np
from .ik_func import motor_num
arch = platform.machine()
print(f"arch is {arch}")
so_path = join(dirname(__file__),'lysdemo_'+arch +'/libControlCAN.so')
lysdemoPath = join(dirname(__file__),'lysdemo_'+arch+'/')
print(f"so_path:{so_path}")

# <<<

'''
 Device Type
'''
LCAN_USBCAN1          = c_uint(3)
LCAN_USBCAN2          = c_uint(4)
LCAN_USBCANFD2        = c_uint(41)
LCAN_USBCANFD1        = c_uint(42)
LCAN_USBCANFDMini     = c_uint(43)
'''
 Interface return status
'''
LCAN_STATUS_ERR         = 0
LCAN_STATUS_OK          = 1

'''
 CAN type
'''
LCAN_TYPE_CAN    = c_uint(0)
LCAN_TYPE_CANFD  = c_uint(1)

LCAN_CANFD_BRS = 0x08
LCAN_CANFD_ESI = 0x10



class LCAN_INIT_CONFIG(Structure):
    _fields_ = [("accCode", c_uint),
                ("accMask", c_uint),
                ("reserved", c_uint),
                ("filter",   c_ubyte),
                ("timing0",  c_ubyte),
                ("timing1",  c_ubyte),
                ("mode",     c_ubyte)]

class LCAN_INITFD_CONFIG(Structure):
    _fields_ = [("abitBaudHz",   c_uint),
                ("dbitBaudHz",   c_uint),
                ("abit_timing",  c_uint),
                ("dbit_timing",  c_uint),
                ("mode",         c_ubyte),
                ("fdEn",         c_ubyte),
                ("isoEn",        c_ubyte),
                ("rev1",         c_ubyte)]

class LCAN_FLITER_CONFIG(Structure):
    _fields_ = [("cmd", c_ubyte),
                ("fliterMode", c_ubyte),
                ("idType", c_ubyte),
                ("rev1", c_ubyte),
                ("fliterStardId", c_uint),
                ("filterEndId", c_uint)]

class LCAN_CAN_OBJ(Structure):
    _fields_ = [("id",            c_uint),
                ("timeStamp",     c_uint),
                ("timeFlag",      c_ubyte),
                ("sendType",      c_ubyte),
                ("remoteFlag",    c_ubyte),
                ("externFlag",    c_ubyte),
                ("dataLen",       c_ubyte),
                ("data",      c_ubyte * 8),
                ("reserved",  c_ubyte * 3)]

class LCAN_CANFD_OBJ(Structure):
    _fields_ = [("id",            c_uint),
                ("timeStamp",     c_uint),
                ("flag",          c_ubyte),
                ("sendType",      c_ubyte),
                ("remoteFlag",    c_ubyte),
                ("externFlag",    c_ubyte),
                ("dataLen",       c_ubyte),
                ("data",      c_ubyte * 64),
                ("reserved",  c_ubyte * 3)]

class LCAN(object):
    def __init__(self):
        if platform.system() == "Linux":
            CDLL(lysdemoPath + "libusb-1.0.so", mode = RTLD_GLOBAL)
            self.__dll = cdll.LoadLibrary(so_path)
        else:
            print(platform.system()+":" +"No support now!")
        if self.__dll == None:
            print("DLL couldn't be loaded!")

    def LCAN_OpenDevice(self, devType, devIndex):
        try:
            return self.__dll.LCAN_OpenDevice(devType, devIndex)
        except:
            print("Exception on LCAN_OpenDevice!")
            raise

    def LCAN_CloseDevice(self, devType, devIndex):
        try:
            return self.__dll.LCAN_CloseDevice(devType, devIndex)
        except:
            print("Exception on LCAN_CloseDevice!")
            raise

    def LCAN_SetFliter(self, devType, devIndex, canIndex, fliter_config):
        try:
            return self.__dll.LCAN_SetFliter(devType, devIndex, canIndex, byref(fliter_config))
        except:
            print("Exception on LCAN_SetFliter!")
            raise

    def LCAN_InitCAN(self, devType, devIndex, canIndex, init_config):
        try:
            return self.__dll.LCAN_InitCAN(devType, devIndex, canIndex, byref(init_config))
        except:
            print("Exception on ZCAN_InitCAN!")
            raise

    def LCAN_InitCANFD(self, devType, devIndex, canIndex, initFD_config):
        try:
            return self.__dll.LCAN_InitCANFD(devType, devIndex, canIndex, byref(initFD_config))
        except:
            print("Exception on LCAN_InitCANFD!")
            raise

    def LCAN_ClearBuffer(self, devType, devIndex, canIndex):
        try:
            return self.__dll.LCAN_ClearBuffer(devType, devIndex, canIndex)
        except:
            print("Exception on LCAN_ClearBuffer!")
            raise
    # type 1 = CANFD; 0 = CAN
    def LCAN_GetReceiveNum(self, devType, devIndex, canIndex, type):
        try:
            return self.__dll.LCAN_GetReceiveNum(devType, devIndex, canIndex, type)
        except:
            print("Exception on LCAN_GetReceiveNum!")
            raise

    def LCAN_Transmit(self, devType, devIndex, canIndex, canObjPtr, len):
        try:
            return self.__dll.LCAN_Transmit(devType, devIndex, canIndex, byref(canObjPtr), len)
        except:
            print("Exception on LCAN_Transmit!")
            raise

    def LCAN_TransmitFD(self, devType, devIndex, canIndex, canFDObjPtr, len):
        try:
            return self.__dll.LCAN_TransmitFD(devType, devIndex, canIndex, byref(canFDObjPtr), len)
        except:
            print("Exception on LCAN_TransmitFD!")
            raise

    def LCAN_Receive(self, devType, devIndex, canIndex, rcv_num, wait_time = c_int(-1)):
        try:
            rcv_can_objs = (LCAN_CAN_OBJ * rcv_num)()
            ret = self.__dll.LCAN_Receive(devType, devIndex, canIndex, byref(rcv_can_objs), rcv_num, wait_time)
            return rcv_can_objs, ret
        except:
            print("Exception on LCAN_Receive!")
            raise

    def LCAN_ReceiveFD(self, devType, devIndex, canIndex, rcv_num, wait_time = c_int(-1)):
        try:
            rcv_canFD_objs = (LCAN_CANFD_OBJ * rcv_num)()
            ret = self.__dll.LCAN_ReceiveFD(devType, devIndex, canIndex, byref(rcv_canFD_objs), rcv_num, wait_time)
            return rcv_canFD_objs, ret
        except:
            print("Exception on LCAN_ReceiveFD!")
            raise
###############################################################################


class MotorController:
    def __init__(self, motornum=6):
        self.motornum = motornum
        self.motor_ids =  range(1, motor_num + 1)
        self._create_control_objects()  # 自动生成所有电机的CAN命令对象
        
    def _create_control_objects(self):
        # 初始化电机命令
        self.can_obj_init = (LCAN_CANFD_OBJ*self.motornum)()

        # 获取状态命令
        self.can_obj_get = (LCAN_CANFD_OBJ*self.motornum)()

        # 设置为位置模式
        self.can_p_mode = (LCAN_CANFD_OBJ*self.motornum)()

        # 设置位置
        self.can_set_p = (LCAN_CANFD_OBJ*self.motornum)()

        # 设置为速度模式
        self.can_v_mode = (LCAN_CANFD_OBJ*self.motornum)()

        # 设置速度
        self.can_set_v = (LCAN_CANFD_OBJ*self.motornum)()

        # 设置为电流模式命令
        self.can_c_mode = (LCAN_CANFD_OBJ*self.motornum)()

        # 电流模式下设置电流大小
        self.can_set_c = (LCAN_CANFD_OBJ*self.motornum)()

        # 清除关节错误
        self.can_clear_err = (LCAN_CANFD_OBJ*self.motornum)()

        # 关节上使能
        self.can_enable = (LCAN_CANFD_OBJ*self.motornum)()

        # 关节下使能
        self.can_disable = (LCAN_CANFD_OBJ*self.motornum)()

        # 获取关节状态
        self.get_state = (LCAN_CANFD_OBJ*self.motornum)()

        # 设置为零点
        self.set_zero = (LCAN_CANFD_OBJ*self.motornum)()
        
        for motor_id in self.motor_ids:
            self.can_obj_init[motor_id-1].id = 0x00 + motor_id  # CAN ID
            self.can_obj_init[motor_id-1].dataLen = 3  # Data length
            self.can_obj_init[motor_id-1].data = (c_ubyte * 64)(0x02, 0x49, 0x00)  # Data bytes

        
            self.can_obj_get[motor_id-1].id = 0x600 + motor_id
            self.can_obj_get[motor_id-1].dataLen = 0
            self.can_obj_get[motor_id-1].data = (c_ubyte * 64)()

            
            self.can_p_mode[motor_id-1].id = 0x00 + motor_id
            self.can_p_mode[motor_id-1].dataLen = 3
            self.can_p_mode[motor_id-1].data = (c_ubyte * 64)(0x02, 0x30, 0x03)

            
            self.can_set_p[motor_id-1].id = 0x200 + motor_id
            self.can_set_p[motor_id-1].dataLen = 4
            self.can_set_p[motor_id-1].data = (c_ubyte * 64)(0x00, 0x00, 0x00, 0x00)


            self.can_v_mode[motor_id-1].id = 0x00 + motor_id
            self.can_v_mode[motor_id-1].dataLen = 3
            self.can_v_mode[motor_id-1].data = (c_ubyte * 64)(0x02, 0x30, 0x02)


            self.can_set_v[motor_id-1].id = 0x300 + motor_id
            self.can_set_v[motor_id-1].dataLen = 4
            self.can_set_v[motor_id-1].data = (c_ubyte * 64)(0x00, 0x00, 0x00, 0x00)


            self.can_c_mode[motor_id-1].id = 0x00 + motor_id
            self.can_c_mode[motor_id-1].dataLen = 3
            self.can_c_mode[motor_id-1].data = (c_ubyte * 64)(0x02, 0x30, 0x01)

            
            self.can_set_c[motor_id-1].id = 0x400 + motor_id
            self.can_set_c[motor_id-1].dataLen = 4
            self.can_set_c[motor_id-1].data = (c_ubyte * 64)(0x00, 0x00, 0x00, 0x00)

            
            self.can_clear_err[motor_id-1].id = 0x00 + motor_id
            self.can_clear_err[motor_id-1].dataLen = 3
            self.can_clear_err[motor_id-1].data = (c_ubyte * 64)(0x02, 0x0f, 0x01)

            
            self.can_enable[motor_id-1].id = 0x00 + motor_id
            self.can_enable[motor_id-1].dataLen = 3
            self.can_enable[motor_id-1].data = (c_ubyte * 64)(0x02, 0x0a, 0x01)

            
            self.can_disable[motor_id-1].id = 0x00 + motor_id
            self.can_disable[motor_id-1].dataLen = 3
            self.can_disable[motor_id-1].data = (c_ubyte * 64)(0x02, 0x0a, 0x00)



            self.set_zero[motor_id-1].id = 0x00 + motor_id
            self.set_zero[motor_id-1].dataLen = 3
            self.set_zero[motor_id-1].data = (c_ubyte * 64)(0x02, 0x0e, 0x01)



    def set_p(self, ctrl_intq):
        for motor_id in self.motor_ids:
            p_des = int(ctrl_intq[motor_id-1])
            hex_p = decimal_to_four_hex_numbers(p_des)
            self.can_set_p[motor_id-1].data = (c_ubyte * 64)(hex_p[0], hex_p[1], hex_p[2], hex_p[3])

    def set_v(self, ctrl_intv):
        for motor_id in self.motor_ids:
            v_des = int(ctrl_intv[motor_id-1])
            hex_v = decimal_to_four_hex_numbers(v_des)
            self.can_set_v[motor_id-1].data = (c_ubyte * 64)(hex_v[0], hex_v[1], hex_v[2], hex_v[3])

    def set_c(self, ctrl_intc):
        for motor_id in self.motor_ids:
            c_des = int(ctrl_intc[motor_id-1])
            hex_c = decimal_to_four_hex_numbers(c_des)
            self.can_set_v[motor_id-1].data = (c_ubyte * 64)(hex_c[0], hex_c[1], hex_c[2], hex_c[3])



    # 把十进制数转换为4个十六进制数，倒序排列
def decimal_to_four_hex_numbers(decimal_number):
    maxdecimal_number = 0xFFFFFFFF
    if decimal_number > maxdecimal_number:
        raise ValueError("Decimal number is too large")
    if decimal_number < 0:
        decimal_number += maxdecimal_number
        
    # 将十进制数转换为4字节的十六进制字符串
    hex_string = f"{decimal_number:08x}"
    
    # 将十六进制字符串拆分为4个两位的十六进制数
    hex_numbers = [int(hex_string[i:i+2], 16) for i in range(0, 8, 2)]
    hex_numbers.reverse()

    return hex_numbers

# >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# 自定义函数



devIndex = 0

canIndex = 0



motor_ids =  range(1, motor_num + 1)


pvc = np.zeros((motor_num,3))

# 初始化 canfd 通信参数
canfd_init_cfg = LCAN_INITFD_CONFIG()
canfd_init_cfg.abitBaudHz = 1000000
canfd_init_cfg.abit_timing = 0x00018B2E
canfd_init_cfg.dbitBaudHz = 5000000
canfd_init_cfg.dbit_timing = 0x00010207
# normal
canfd_init_cfg.mode = 0
# CANFD enabled
canfd_init_cfg.fdEn = 1
# ISO CANFD enabled
canfd_init_cfg.isoEn = 1

canfd_init_cfg.rev1 = 0



# def receive_print(lcanlib, devIndex, canIndex):
#     # 变量lcanlib = LCAN()
#     can_buff_num = lcanlib.LCAN_GetReceiveNum(LCAN_USBCANFD2, devIndex, canIndex, 1)
#     print("CAN Buffer Num: ", can_buff_num)
#     if can_buff_num > 0:
#         rcv_canFD_objs, ret = lcanlib.LCAN_ReceiveFD(LCAN_USBCANFD2, devIndex, canIndex, can_buff_num, 1000)
#         for i in range(can_buff_num):
#             print(f"num {i}: CANFD_ID {hex(rcv_canFD_objs[i].id)}\n")
#             print(f"num {i}: CANFD_Data_Len {hex(rcv_canFD_objs[i].dataLen)}\n")

#             data_hex = [hex(byte) for byte in rcv_canFD_objs[i].data[:rcv_canFD_objs[i].dataLen]]
#             data = [byte for byte in rcv_canFD_objs[i].data[:rcv_canFD_objs[i].dataLen]]
#             print(f"num {i}: CANFD_Data {data_hex}\n")
#             print(f"num {i}: CANFD_Data {data}\n")





# 获得电机p, v, c状态
def motor_pvc(rcv_canFD_objs, i):
    can_id = rcv_canFD_objs[i].id
    data = [byte for byte in rcv_canFD_objs[i].data[:rcv_canFD_objs[i].dataLen]]

    if 0x500 < can_id <= 0x500 + motor_num:
        ID = can_id - 0x500
        # print(f"can id is 0x{500+ID}")
        c = data[3] * 16777216 + data[2] * 65536 + data[1] * 256 + data[0] # 256**3 256**2 256
        v = data[7] * 16777216 + data[6] * 65536 + data[5] * 256 + data[4]
        p = data[11] * 16777216 + data[10] * 65536 + data[9] * 256 + data[8]

        # enable_code = [data[12], data[13]]
        # error_code = [data[14], data[15]]

        # if enable_code[0] == 0x01:
        #     print(f"motor {ID} is enable")
        # else:
        #     print(f"motor {ID} is disable")

        # print(f"error code is {error_code}")

        if c > 2147483648: #round((256**4)/2)
            c -= 4294967296
        if v > 2147483648:
            v -= 4294967296
        if p > 2147483648:
            p -= 4294967296

        pvc[ID - 1][0] = p
        pvc[ID - 1][1] = v
        pvc[ID - 1][2] = c
    return pvc


# 读取0x500反馈
def receive_pvc(lcanlib, devIndex, canIndex):
    # 变量lcanlib = LCAN()
    can_buff_num = lcanlib.LCAN_GetReceiveNum(LCAN_USBCANFD2, devIndex, canIndex, 1)
    # print("CAN Buffer Num: ", can_buff_num)
    if can_buff_num > 0:
        rcv_canFD_objs, ret = lcanlib.LCAN_ReceiveFD(LCAN_USBCANFD2, devIndex, canIndex, can_buff_num, 1000)
        for i in range(can_buff_num):
            pvc = motor_pvc(rcv_canFD_objs, i)
        return pvc
    else:
        return None


p_state = np.zeros(motor_num)
# 读取0x700反馈
def receive_p_state(lcanlib, devIndex, canIndex):
    # 变量lcanlib = LCAN()
    can_buff_num = lcanlib.LCAN_GetReceiveNum(LCAN_USBCANFD2, devIndex, canIndex, 1)
    # print("CAN Buffer Num: ", can_buff_num)
    if can_buff_num > 0:
        rcv_canFD_objs, ret = lcanlib.LCAN_ReceiveFD(LCAN_USBCANFD2, devIndex, canIndex, can_buff_num, 1000)
        for i in range(can_buff_num):

            can_id = rcv_canFD_objs[i].id
            data = [byte for byte in rcv_canFD_objs[i].data[:rcv_canFD_objs[i].dataLen]]

            if 0x700 < can_id <= 0x700 + motor_num:
                ID = can_id - 0x700
                print(f"get ID is {ID}")
                p_state[ID - 1] = data[11] * 16777216 + data[10] * 65536 + data[9] * 256 + data[8]
                if p_state[ID - 1] > 2147483648: #round((256**4)/2)
                    p_state[ID - 1] -= 4294967296
        return p_state
    else:
        return None




